#include <tasks_manager/msg_convert.h>

/*******************************************
 * 时间戳转换
 *******************************************/

// 时间戳转标准字符串格式
string MsgConvert::TimestampFormat(long timestamp){
    std::ostringstream oss;
    oss << std::put_time(localtime(&(timestamp)), "%Y-%m-%d %H:%M:%S");
    return oss.str();
}

// 时间字符串转时间戳
std::time_t MsgConvert::TimeStr2Timestamp(string time_str){
    std::tm tm = {};
    std::istringstream ss(time_str);
    ss >> std::get_time(&tm, "%Y-%m-%d %H:%M:%S");
    if(ss.fail()) {
        cout << "[TimeStr2Timestamp] Failed to parse time string" <<  endl;
        return 0;
    }
    return std::mktime(&tm);
}


// 获取时区
std::string MsgConvert::GetTimezone() {
    std::ifstream tzFile("/etc/timezone");
    std::string timezone;
    if (tzFile.is_open()) {
        std::getline(tzFile, timezone);
        tzFile.close();
    } else {
        throw std::runtime_error("Failed to open /etc/timezone");
    }
    return timezone;
}

/*******************************************
 * ROS默认消息数据转换
 *******************************************/

// Euler(roll, pitch, yaw) -> tf2::Quaternion
tf2::Quaternion MsgConvert::Euler2TFQuaternion(double roll, double pitch, double yaw){
    tf2::Quaternion tf2_quat;
    tf2_quat.setRPY(roll, pitch, yaw);
    return tf2_quat;
}

// tf2::Quaternion -> Euler(roll, pitch, yaw)
std::tuple<double, double, double> MsgConvert::TFQuaternion2Euler(tf2::Quaternion tf2_quat){
    double roll, pitch, yaw;
    tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw);
    return std::make_tuple(roll, pitch, yaw);
}

// Euler(roll, pitch, yaw) -> geometry_msgs::Quaternion
geometry_msgs::Quaternion MsgConvert::Euler2GeometryQuaternion(double roll, double pitch, double yaw){
    tf2::Quaternion tf2_quat = Euler2TFQuaternion(roll, pitch, yaw);
    geometry_msgs::Quaternion geom_quat = tf2::toMsg(tf2_quat);
    return geom_quat;
}

// geometry_msgs::Quaternion -> Euler(roll, pitch, yaw)
std::tuple<double, double, double> MsgConvert::GeometryQuaternion2Euler(geometry_msgs::Quaternion geom_quat){
    tf2::Quaternion tf2_quat;
    tf2::fromMsg(geom_quat, tf2_quat);
    return TFQuaternion2Euler(tf2_quat);
}

// geometry_msgs::Pose -> geometry_msgs::Point
geometry_msgs::Point MsgConvert::Pose2Point(geometry_msgs::Pose pose){
    geometry_msgs::Point point;
    point.x = pose.position.x;
    point.y = pose.position.y;
    point.z = pose.position.z;
    return point;
}

// vector<geometry_msgs::Pose> -> vector<geometry_msgs::Point>
vector<geometry_msgs::Point> MsgConvert::Poses2Points(vector<geometry_msgs::Pose> poses){
    vector<geometry_msgs::Point> points;
    for(auto& pose : poses){
        points.emplace_back(Pose2Point(pose));
    }
    return points;
}

/*******************************************
 * 转换成字符串
 *******************************************/
// Bool -> string
string MsgConvert::Bool2Str(bool data){
    return data ? "true" : "false";
}

// string -> bool
bool MsgConvert::Str2Bool(string data){
    if(data == "true") return true;
    else if(data == "false") return false;
    return false;
}

// 特殊字符转义
std::string MsgConvert::EscapeString(const std::string& input){
    std::string result;
    for (char c : input) {
        if (c == '\'' || c == '\"' || c == '\\') {
            result += '\\';
        }
        result += c;
    }
    return result;
}

/*******************************************
 * JSON 消息数据转换
 *******************************************/
// 检测Json字段是否有效
bool MsgConvert::CheckJsonItemIsValid(Json::Value& json_data, string item){
    if(json_data.isMember(item) && !json_data[item].isNull()){
        return true;
    }
    return false;
}

// 消息转换 Json::Value -> std::string
string MsgConvert::JsonValue2String(Json::Value json_data){
    return json_data.toStyledString();
}

// 消息转换 std::string -> Json::Value
Json::Value MsgConvert::String2JsonValue(string str_data){
    Json::Reader reader;
    Json::Value json_data;
    reader.parse(str_data, json_data); 
    return json_data;
}

// geometry_msgs::Point -> Json::Value
Json::Value MsgConvert::Point2Json(geometry_msgs::Point& point){
    Json::Value json_data;
    json_data["x"] = point.x;
    json_data["y"] = point.y;
    json_data["z"] = point.z;
    return json_data;
}

// Json::Value -> geometry_msgs::Point
geometry_msgs::Point MsgConvert::Json2Point(Json::Value& json_data){
    geometry_msgs::Point point;
    point.x = json_data["x"].asFloat();
    point.y = json_data["y"].asFloat();
    point.z = json_data["z"].asFloat();
    return point;
}

// geometry_msgs::Quaternion -> Json::Value
Json::Value MsgConvert::Quaternion2Json(geometry_msgs::Quaternion& q){
    Json::Value json_data;
    json_data["x"] = q.x;
    json_data["y"] = q.y;
    json_data["z"] = q.z;
    json_data["w"] = q.w;
    return json_data;
}

// Json::Value -> geometry_msgs::Quaternion
geometry_msgs::Quaternion MsgConvert::Json2Quaternion(Json::Value& json_data){
    geometry_msgs::Quaternion q;
    q.x = json_data["x"].asFloat();
    q.y = json_data["y"].asFloat();
    q.z = json_data["z"].asFloat();
    q.w = json_data["w"].asFloat();
    return q;
}

// geometry_msgs::Pose -> Json::Value
Json::Value MsgConvert::Pose2Json(geometry_msgs::Pose& pose){
    Json::Value json_data;
    json_data["position"] = Point2Json(pose.position);
    // json_data["orientation"] = Quaternion2Json(pose.orientation);        //数据量较大，暂时屏蔽方向数据
    return json_data;
}

// Json::Value -> geometry_msgs::Pose
geometry_msgs::Pose MsgConvert::Json2Pose(Json::Value& json_data){
    geometry_msgs::Pose pose;
    pose.position = Json2Point(json_data["position"]);
    // pose.orientation = Json2Quaternion(json_data["orientation"]);        //数据量较大，暂时屏蔽方向数据
    return pose;
}

// ros::Time -> Json::Value
Json::Value MsgConvert::RosTime2Json(ros::Time& time){
    Json::Value json_data;
    json_data["sec"] = time.sec;
    json_data["nsec"] = time.nsec;
    return json_data;  
}

// Json::Value -> ros::Time
ros::Time MsgConvert::Json2RosTime(Json::Value& json_data){
    ros::Time time;
    time.sec = json_data["sec"].asInt64();
    time.nsec = json_data["nsec"].asInt64();
    return time;  
}

// nav_msgs::MapMetaData -> Json::Value
Json::Value MsgConvert::MapMetaData2Json(nav_msgs::MapMetaData& metadata){
    Json::Value json_data;
    json_data["map_load_time"] = RosTime2Json(metadata.map_load_time);
    json_data["resolution"] = metadata.resolution;
    json_data["width"] = metadata.width;
    json_data["height"] = metadata.height;
    json_data["origin"] = Pose2Json(metadata.origin);
    return json_data;
}

// Json::Value -> nav_msgs::MapMetaData
nav_msgs::MapMetaData MsgConvert::Json2MapMetaData(Json::Value& json_data){
    nav_msgs::MapMetaData metadata;
    metadata.map_load_time = Json2RosTime(json_data["map_load_time"]);
    metadata.resolution = json_data["resolution"].asFloat();
    metadata.width = json_data["width"].asInt64();
    metadata.height = json_data["height"].asInt64();
    metadata.origin = Json2Pose(json_data["origin"]);
    return metadata;
}

// std_msgs::Header -> Json::Value
Json::Value MsgConvert::Header2Json(std_msgs::Header& header){
    Json::Value json_data;
    json_data["stamp"] = RosTime2Json(header.stamp);
    json_data["seq"] = header.seq;
    json_data["frame_id"] = header.frame_id;
    return json_data;
}

// Json::Value -> std_msgs::Header
std_msgs::Header MsgConvert::Json2Header(Json::Value& json_data){
    std_msgs::Header header;
    header.stamp = Json2RosTime(json_data["stamp"]);
    header.seq = json_data["seq"].asInt64();
    header.frame_id = json_data["frame_id"].asString();
    return header;
}

// geometry_msgs::PoseStamped -> Json::Value
Json::Value MsgConvert::PoseStamped2Json(geometry_msgs::PoseStamped& pose_stamp){
    Json::Value json_data;
    json_data["header"] = Header2Json(pose_stamp.header);
    json_data["pose"] = Pose2Json(pose_stamp.pose);
    return json_data;
}

// Json::Value -> geometry_msgs::PoseStamped
geometry_msgs::PoseStamped MsgConvert::Json2PoseStamped(Json::Value& json_data){
    geometry_msgs::PoseStamped pose_stamp;
    pose_stamp.header = Json2Header(json_data["header"]);
    pose_stamp.pose = Json2Pose(json_data["pose"]);
    return pose_stamp;
}

// nav_msgs::Path -> Json::Value
Json::Value MsgConvert::Path2Json(nav_msgs::Path& path){
    Json::Value json_data;
    json_data["header"] = Header2Json(path.header);
    Json::Value json_poses;
    for(auto pose : path.poses){
        json_poses.append(PoseStamped2Json(pose));
    }
    json_data["poses"] = json_poses;
    return json_data;
}

// Json::Value -> nav_msgs::Path
nav_msgs::Path MsgConvert::Json2Path(Json::Value& json_data){
    nav_msgs::Path path;
    if(CheckJsonItemIsValid(json_data, "header"))
        path.header = Json2Header(json_data["header"]);
    if(CheckJsonItemIsValid(json_data, "poses")){
        for(auto json_pose : json_data["poses"]){
            path.poses.emplace_back(Json2PoseStamped(json_pose));
        }
    }
    return path;
}

// vector<geometry_msgs::Pose> -> Json::Value
Json::Value MsgConvert::Poses2Json(vector<geometry_msgs::Pose>& poses){
    Json::Value poses_data;
    for(auto& pose : poses){
        poses_data.append(Pose2Json(pose));
    }
    return poses_data;
}

// Json::Value -> vector<geometry_msgs::Pose>
vector<geometry_msgs::Pose> MsgConvert::Json2Poses(Json::Value& json_data){
    vector<geometry_msgs::Pose> poses;
    for(auto json_pose : json_data){
        poses.emplace_back(Json2Pose(json_pose));
    }
    return poses;
}


// vector<int> -> Json::Value
Json::Value MsgConvert::IntVector2Json(vector<int>& int_data){
    Json::Value json_data;
    for(auto& data : int_data){
        json_data.append(data);
    }
    return json_data;
}

// Json::Value -> vector<int>
vector<int> MsgConvert::Json2IntVector(Json::Value& json_data){
    vector<int> int_data;
    for(auto& json_item : json_data){
        int_data.emplace_back(json_item.asInt());
    }
    return int_data;
}
